/********************************************************************************
 * Copyright(c) 2020-2025 BST All rights reserved.
 * - Filename
 * - Author
 * - Version V1.0.0
 * - Date    2024/08/28
 * - Brief
 * - FunctionList:
 ******************************************************************************
 * History:
 *
 *
 *
 ******************************************************************************
 */
#include "RsLidarAdapter.h"

#include <sys/time.h>

#include "TimeUtil.h"

using namespace robosense::lidar;

RsLidarAdapter::RsLidarAdapter(const uint32_t &id, const std::string &ip,
                               const uint16_t &msop_port,
                               const uint16_t &difop_port,
                               const std::string &model)
    : _id(id),
      _ip(ip),
      _msopPort(msop_port),
      _difopPort(difop_port),
      _model(model) {
  std::string info;
  info.append("Id:");
  info.append(std::to_string(_id));
  info.append(", model:");
  info.append(_model);
  info.append(", IP:");
  info.append(_ip);
  info.append(", MsopPort:");
  info.append(std::to_string(_msopPort));
  info.append(", DifoPort:");
  info.append(std::to_string(_difopPort));
  _modelMap.insert({"rs16", int(LidarType::RS16)});
  _modelMap.insert({"rs32", int(LidarType::RS32)});
  _modelMap.insert({"rsbp", int(LidarType::RSBP)});
  _modelMap.insert({"rs128", int(LidarType::RS128)});
  _modelMap.insert({"rs12840", int(LidarType::RS128_40)});
  _modelMap.insert({"rs80", int(LidarType::RS80)});
  _modelMap.insert({"rshelios", int(LidarType::RSHELIOS)});
  _modelMap.insert({"rsrock", int(LidarType::RSROCK)});
  _modelMap.insert({"rsm1", int(LidarType::RSM1)});
  std::cout << "RsLidarAdapter  create : " << info << std::endl;
}

RsLidarAdapter::~RsLidarAdapter() {
  std::cout << "RsLidarAdapter Destroy..." << std::endl;
}

void RsLidarAdapter::start(
    std::function<void(std::shared_ptr<lpc::LidarProtoPointCloud> &&)>
        callback) {
  _isExit = false;
  _lidar = std::make_shared<LidarDriver<RsPointXYZI>>();
  _lidarConfig = std::make_shared<RSDriverParam>();
  _lidarConfig->input_param.msop_port = _msopPort;
  _lidarConfig->input_param.difop_port = _difopPort;
  _lidarConfig->lidar_type = LidarType::RSHELIOS;
  _lidarConfig->print();
  _lidar->regExceptionCallback([this](const robosense::lidar::Error &err) {
    this->exceptionCallback(err);
  });
  _lidar->regRecvCallback(
      [this](const robosense::lidar::PointCloudMsg<RsPointXYZI> &msg) {
        this->lidarFrameCallback(msg);
      });

  if (!_lidar->init(*_lidarConfig)) {
    std::cout << "RsLidarAdapter init error !! " << std::endl;
    return;
  }
  _lidar->start();
  _lidarFrameThread.reset(
      new std::thread(std::bind(&RsLidarAdapter::processLoop, this, callback)));
}

void RsLidarAdapter::stop() {
  _lidar->stop();
  _isExit = true;
  _condition.notify_all();
  if (_lidarFrameThread != nullptr && _lidarFrameThread->joinable()) {
    _lidarFrameThread->join();
  }
  std::cout << "RsLidarAdapter Exit ..." << std::endl;
}

void RsLidarAdapter::processLoop(
    const std::function<void(std::shared_ptr<lpc::LidarProtoPointCloud> &&)>
        &callback) {
  while (!_isExit) {
    std::unique_lock<std::mutex> lck(_mtx);
    _condition.wait(lck,
                    [this] { return _isExit || !_lidarFrameDeque.empty(); });
    if (_isExit) break;
    auto lidarFrame = _lidarFrameDeque.front();
    _lidarFrameDeque.pop_front();
    lck.unlock();
    if (_isExit) break;

    callback(std::move(lidarFrame));
  }
}

void RsLidarAdapter::lidarFrameCallback(const PointCloudMsg<RsPointXYZI> &msg) {
  auto frame = std::make_shared<lpc::LidarProtoPointCloud>();
  frame->set_lidar_id(_id);
  frame->set_system_timestamp(TimeUtil::now_us());
  frame->set_timestamp((uint64_t)(msg.timestamp * 1000000));  // double s to us
  frame->set_seq(msg.seq);
  frame->set_height(msg.point_cloud_ptr->size());
  frame->set_width(1);
  frame->set_is_dense(false);

  for (int i = 0; i < (int)msg.point_cloud_ptr->size(); i++) {
    frame->add_data(msg.point_cloud_ptr->at(i).x);
    frame->add_data(msg.point_cloud_ptr->at(i).y);
    frame->add_data(msg.point_cloud_ptr->at(i).z);
    frame->add_data(msg.point_cloud_ptr->at(i).intensity);
  }

  {
    std::unique_lock<std::mutex> lck(_mtx);
    _lidarFrameDeque.emplace_back(frame);
  }
  _condition.notify_one();
}
void RsLidarAdapter::exceptionCallback(const Error &code) {}
